/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Estimates the rotation needed to minimize the RMSE
* between GPS and laser odometry trajectories.
* Writes the rotated points to file "RotatedPoints.txt".
*
*  @author 	Juhana 
*
**/

#ifndef ESTIMATEROTATION_H_
#define ESTIMATEROTATION_H_

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include "owndebug.h"
#include "ownutils.h"
#include "gimutils.h"
#include "PositionFileReader.hpp"
//#include "CScanMeasurementSet.h"

/************************************************************************************/
using namespace std;

struct Point {
unsigned int s, us;
float X,Y, angle;
} typedef TFPoint;

/**
* A Least Squares optimisation based on matched pointpairs
* The pairs should be ordered so that refpoint[i] corresponds to actualpoint[i].
* @param RefPoints[]    List of points in reference scan
* @param ActualPoints[] List of points in the current measurement
* @param N              Number of point pairs
* @param *Tx            @out Translation in x-dir
* @param *Ty            @out Translation in y-dir
* @param *fi            @out Rotation in heading
  **/
void MatchPairPoints(TFPoint RefPoints[], TFPoint ActualPoints[], int N, float *Tx, float *Ty, float *fi);

/**
* Reads two pose files in MaCI format
* @param *positionFile1		Posefile1
* @param *positionFile2		Posefile2
* @param &N								Length of pose files
* @param &counter2				length of the original Matched-file
**/
bool readTwoPoseFiles(const char *positionFile1, const char *positionFile2, int &N, int &counter2);

/**
* Rotates points fi radians clockwise around the first point in the file
* @param poses[]	Points to be rotated
* @param Tx				offset in x-direction	
* @param Ty				offset in y-direction
* @param fi				Amount of clockwise rotation in radians
* @param N			  Length of pose file
**/
bool 	RotatePoints(TFPoint poses[], float Tx, float Ty, float fi, int N);

/**
* Writes points to File in MaCI format
* @params poses[]		Poses to be written
* @params N 				Lenger of pose file
**/
bool WritePointsToFile(TFPoint poses[], int N);

/// pointers for poses
TFPoint* poses1 = NULL;	// GPS points
TFPoint* poses2 = NULL; // Matched points
TFPoint* poses3 = NULL; // Original matched points
TFPoint* poses4 = NULL; // Rotated points
   

#endif

